机械臂示教功能开发

修订日期	修订版本	修订内容	修订人
2022.09.29	V0.1	初始化文档	赵锦强

[TOC]

机械臂示教时在速度模式下运动。通常一次示教包括加速、匀速、减速过程。示教时依次调用以下接口:

初始化示教起点(因为示教运动处于速度模式，每次结束时机械臂的起始运动位置是不确定的，所以需要先更新机械臂起始关节位置)

/**
     * @brief tpInitial: 初始化规划器状态
     * @param joint: 机械臂规划起始关节角
     * @param refPoint: 参考轨迹点信息(可以利用 RefTrajectoryPoint 中的指针变量直接访问规划结果)
     */
    ARAL_API_COMMON(1.0) void tpInitiatePlanner(const RLJntArray& joint, RefTrajectoryPoint& refPoint) = 0;

//使用示例
ARAL::interface::RLJntArray joint;
memcpy(joint.data(), getCurJointPosition().data(), sizeof (double) * dof_);
RefTrajectoryPoint temp;
aral_interface_->tpInitiatePlanner(joint, temp);

向规划器中添加速度运动（关节空间或笛卡尔空间）

/**
     * @brief 向规划器中添加速度运动（关节空间或笛卡尔空间）
     * @param vel: 目标运动速度
     * @param id: 速度运动对应的路径id
     * @param path_property: 路点对应的几何属性
     * @param move_property: 路点对应的运动属性
     * @return: 返回值 < 0 表示添加失败
     *          E_PLN_OK:添加成功
     *          E_PLN_INVALID_MAX_JOINT_VEL: 关节空间目标运动速度维度错误
     *          E_PLN_INVALID_CATERSIAN_TARGET_SPEED: 笛卡尔空间目标运动速度维度错误
     *          E_PLN_CAL_VEL_PROFILE_FAILED: 速度规划失败，可能输入的始末速度、速度约束条件不对
     *          E_PLN_INVALID_ROADPOINT: 无效路点，规划的起始点和当前机械臂关节角不一致，一般不会触发
     *          E_PLN_TRAJ_INERST_FAILED: 规划器处于停止中或者暂停状态无法插入路径
     *          E_PLN_EXCEED_MAX_TRAJECTORY_UNIT_SIZE: 超出轨迹队列允许的最大轨迹单元数2
     */
    ARAL_API_COMMON(1.0) int tpAddVelocityLine(const std::vector<double>& vel, const int& id, const PathProperty& path_property, const MoveProperty& move_property) = 0;

//使用示例
ARAL::interface::PathProperty path_property;
ARAL::interface::MoveProperty move_property;
path_property.describe_space = interface::DescribeSpace::CARTESIAN;   //笛卡尔空间
move_property.moveMode = 1;            //速度模式
move_property.moveDuration = 100;      //运动时间
...                                    //其他运动属性设置
std::vector<double> xd = {0, -0.1, 0.2, 0.3, 0, 0}; //Y轴速度-0.1m/s,Z轴速度0.2m/s，Rx速度0.3rad/s
int retval = aral_interface_->tpAddVelocityLine(xd, -1, path_property, move_property);

停止示教，调用tpStop接口，机械臂减速到零

/**
     * @brief 终止轨迹段的执行, 并减速停止
     * @param type: 减速类型,具体参考StopType枚举；除暂停类型外，其他停止类型只保留轨迹执行队列第一个轨迹单元，其余清除
     * @param move_property: 最大速度, 加速度和 jerk 设置, 其维度要与关节空间或笛卡尔空间维度一致；也可不做设置, 采用当前轨迹段的加速度
     * @param buffSize: 减速时缓冲区的大小, 如果软件内部有缓冲处理, 可以将该参数置零(建议在软件内部通过调用tpUpdateCycle函数来缓冲数据点)
     * @return: 返回值 < 0 表示状态切换失败
     *          E_PLN_INVALID_DECELERATION: 采用用户设置的减速度，减速度设置的过小
     *          E_PLN_CAL_VEL_PROFILE_FAILED: 速度规划失败，可能输入的始末速度、速度约束条件不对
     */
    ARAL_API_COMMON(1.0) int tpStop(const StopType& type, const MoveProperty& move_property, const unsigned int buffSize) = 0;

//使用示例
aral_interface_->tpStop(type, move_property, 1);

具体的运动控制逻辑可以参考机器人算法仿真验证软件平台中的代码，在robot_control.cpp的motionControlThread函数:

   int RobotControl::motionControlThread()
   {
       /**
        * @brief 流程
        * 1. 获取当前机械臂各关节的状态(先更新机械臂关节位置，然后更新传感器的数值)
          2. 更新控制目标(先设置目标力, 再设置目标位置)
          3. 计算控制命令
          4. 通过driver发送控制命令
          5. 记录数据
        */
       int  ret;
       //!更新当前状态(包括机械臂关节和传感器状态)
       if((ret = updateRobotStatus()) != E_SVSP_OK)
       {
           LOGW(log_) << "发生错误, 进入紧急停止程序" << ENDL(log_);
           return ret;
       }

       //! 更新控制目标
       if((ret = updateRobotGoal()) < E_SVSP_OK)    //设置运动目标(包括参考轨迹和机械臂在力控坐标系下输出的wrench)
       {
           LOGE(log_) << "轨迹规划计算失败, 结束线程，错误码:" << ret << ENDL(log_);
           return ret;
       }

       //! 计算控制输出
       if ((ret = getRobotOutput()) < 0)  //计算控制输出:机械臂关节轨迹或者驱动力矩
       {
          LOGE(log_) << "控制命令计算失败, 错误码:" << ret << ENDL(log_);
          return ret;
       }

       //! 通过driver发送控制命令
       if(control_mode_ == ControlMode::Position) // 位置控制
           ret = sendJointCommand(cmd_joint_pos_, cmd_joint_vel_, cmd_joint_acc_);
       else    // 力控制
           ret = sendJointCommand(cmd_joint_cur_, {}, {});

       return ret;
   }

3.1.机械臂示教功能开发

机械臂示教功能开发

results matching ""

No results matching ""